/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "gimicore.h"

namespace gimi
{

  GIMICore::GIMICore(int defBufferSize,
                     const std::string cDescription,
                     const std::string cLocation,
                     const int cVersion,
                     const int cBuild)
    : CSync(4,4),
      iConnector(new Connector(this)),
      messageBuffers (defBufferSize, &activeStatus, iConnector),
      messageFilter (),
      subscribers (),
      replyHandler (),
      subscriptions (),
      iServiceDiscoveryEventArray(),
      iServiceListEventArraySeq(),
      iServiceDiscoveryBundleArray(),
      iDefaultConnectorTimeout_ms(1000),
      activeStatus (true),
      iUniqueIdentifier (1),
      defaultBufferSize (defBufferSize),
      hubAddress (),
      hubPort (0),
      ownHubName (),
      clientDescription (cDescription),
      clientLocation (cLocation),
      clientVersion (cVersion, cBuild),
      handleUpkeepThread (NULL)
  {
    dPrint(30, "Called");
    assert(defBufferSize > -1);

  
    handleUpkeepThread = ownThread_Create((void *)(upkeepThread), this);
  
    assert(handleUpkeepThread != NULL); // Verify that upkeep-thread started.
  }

  GIMICore::~GIMICore()
  {
    void *param;

    activeStatus.deactivate(); // Tell all timeouting methods to stop.
    //iConnector->activeStatus.deactivate(); // Tell datareceiverthread to stop.
    iConnector->stop(); // Tell datareceiverthread to stop.


    // Wait upkeepThread.
    if (handleUpkeepThread) {
      //dPrint(ODTEST, "Waiting for upkeepThread to quit.");
      ownThread_Join(handleUpkeepThread, &param); // Wait for upkeep thread to stop.
      handleUpkeepThread = NULL;
      //dPrint(ODTEST, "upkeepThread joined.");
    }

    // Wake up methods in case they are waiting for a condition.
    messageBuffers.broadcastCond();
    ownCond_Broadcast(replyHandler.checkReplyCondHandle);

    delete iConnector;
    iConnector = NULL;

    //dPrint(ODTEST, "GIMICore destructed");
  }

  bool GIMICore::readSettings(const gim::CSettings &settings)
  {
    bool result = true;
    std::string basekey;
    std::string valuestring, valuestring2;
    int value, value2, value3, loopCount = 1;
    int cVersion = 0, cBuild = 0, mCount = 0, n = 0;
    char loopCountString[24];
  
    basekey = "GIMI.";
  
    // Get basic settings.
    if (settings.GetValue(basekey + "queuesize", value)) {
      defaultBufferSize = value;
      dPrint(30, "Default buffersize set to %d", value);
    }
    if (settings.GetValue(basekey + "description", valuestring)) {
      clientDescription = valuestring;
      dPrint(30, "Description set to %s", valuestring.c_str());
    }
    if (settings.GetValue(basekey + "location", valuestring)) {
      clientLocation = valuestring;
      dPrint(30, "Location set to %s", valuestring.c_str());
    }
    if (settings.GetValue(basekey + "version", value)) {
      cVersion = value;
      dPrint(30, "Got version %d", value);
    }
    if (settings.GetValue(basekey + "build", value)) {
      cBuild = value; 
      dPrint(30, "Got build %d", value);
    }
    if (settings.GetValue(basekey + "hubaddress", valuestring)) {
      hubAddress = valuestring;
      dPrint(30, "Hub address set to %s", valuestring.c_str());
    }
    if (settings.GetValue(basekey + "hubport", value)) {
      hubPort = value;
      dPrint(30, "Hub port set to %d", value);
    }
    if (settings.GetValue(basekey + "ownhubname", valuestring)) {
      ownHubName = valuestring;
      dPrint(30, "Own hubname set to %s", valuestring.c_str());
    }
  
    if (cVersion != 0 || cBuild != 0) {
      if (clientVersion.setVersion(cVersion, cBuild)) {
        dPrint(30, "Clientversion set to %s",
               clientVersion.getVersionString().c_str());
      }
    }
  
    // Set queue sizes.
    loopCount = 1;
    result = true;
    while(result) {
      snprintf(loopCountString, 24, "%d", loopCount);
      basekey = "GIMI.queuesizes.queue";
      basekey += loopCountString;
      basekey += ".";
    
      valuestring = "";
      valuestring2 = "";
      if (settings.GetValue(basekey + "majortype", value) &&
          settings.GetValue(basekey + "minortype", value2) &&
          settings.GetValue(basekey + "size", value3))
        {
          // Set size for queue.
          messageBuffers.setQueueMaxSize(value, value2, value3);
            
          ++loopCount;
          dPrint(30, "Set new maximum size %d for type %d:%d", value3, value,
                 value2);
        } else {
        dPrint(30, "Setting queue sizes, did not find majortypeid, " 
               "minortypeid and size in %s", basekey.c_str());
        result = false;
      }
    }
  
    // Add provided services.
    loopCount = 1;
    result = true;
    while(result) {
      snprintf(loopCountString, 24, "%d", loopCount);
      basekey = "GIMI.providedservices.service";
      basekey += loopCountString;
      basekey += ".";

      valuestring = "";
      valuestring2 = "";
      value3 = 0;
      if (settings.GetValue(basekey + "majortype", value) &&
          settings.GetValue(basekey + "minortype", value2))
        {
          settings.GetValue(basekey + "description", valuestring);
          settings.GetValue(basekey + "location", valuestring2);
          settings.GetValue(basekey + "keepalive", value3);
      
          if (settings.GetValue(basekey + "minorcount", mCount)) {
            int minorValue = value2;
            for (n = 0; n < mCount; ++n) {
              subscribers.addServiceType(value, minorValue, valuestring, valuestring2, value3);
              ++minorValue;
            }
            dPrint(30, "Added %d provided services, starting from %d:%d",
                   n+1, value, value2);
          } else {
            // Add service to GIMI.
            subscribers.addServiceType(value, value2, valuestring, valuestring2, value3);
            dPrint(30, "Added a provided service %d:%d %s @ %s with keepalive %d", value, value2,
                   valuestring.c_str(), valuestring2.c_str(), value3);
          }
      
          ++loopCount;
        } else {
        dPrint(30, "Adding provided services, did not find majortypeid and " 
               "minortypeid in %s", basekey.c_str());
        result = false; 
      }
    }
  
    // Add accepted services.
    loopCount = 1;
    result = true;
    while(result) {
      snprintf(loopCountString, 24, "%d", loopCount);
      basekey = "GIMI.acceptedservices.service";
      basekey += loopCountString;
      basekey += ".";
    
      valuestring = "";
      valuestring2 = "";
      if (settings.GetValue(basekey + "majortype", value) &&
          settings.GetValue(basekey + "minortype", value2))
        {
          settings.GetValue(basekey + "description", valuestring);
          settings.GetValue(basekey + "location", valuestring2);
      
          // Add wanted type.
          messageFilter.addWantedType(value, value2, valuestring, valuestring2);
            
          ++loopCount;
          dPrint(30, "Added an accepted service %d:%d %s @ %s", value, value2,
                 valuestring.c_str(), valuestring2.c_str());
        } else {
        dPrint(30, "Adding accepted services, did not find majortypeid and " 
               "minortypeid in %s", basekey.c_str());
        result = false;
      }
    }
    
    return true;
  }

  int GIMICore::connectToHub(const std::string &hAddress /* = "" */,
                             const int hPort /* = -1 */,
                             const std::string &oName /* = "" */,
                             const bool aForceNameRegistration,
                             const std::string &aSecretKey,
                             const int aTimeout_ms)
  {
    int result = GIMI_ERROR;
    dPrint(10,"Called with %s:%d '%s'", hAddress.c_str(), hPort,
           oName.c_str());
  
    // Write new settings if caller specifies.
    if (!hAddress.empty())
      hubAddress = hAddress;
  
    if (hPort != -1)
      hubPort = hPort;
  
    if (!oName.empty())
      ownHubName = oName;

    result = iConnector->connectToHub(hubAddress, hubPort, ownHubName,
                                      aForceNameRegistration,
                                      (const uint8_t *)aSecretKey.c_str(), 
                                      aSecretKey.size(), 
                                      aTimeout_ms);
  
    if (result == GIMI_OK) {
      /*      dPrint(7,"Connected! Our id is 0x%08x (%d)", iConnector->getHubId(),
             iConnector->getHubId());
      */
      //TODO Move to some initializer?
      // Add client description.
      if (clientLocation.empty()) {
        char hostname[256];
        if (gethostname(hostname, 256) == 0) {
          clientLocation = hostname;
        } else {
          dPrint(2, "gethostname() failed, could not set location to hostname.");
        }
      }
    
      // Add description and location information
      subscribers.addServiceType(0, 0, clientDescription, clientLocation);
    }
  
    //ownMutex_Unlock(nifOutMutex);
  
    return result;
  }

  int GIMICore::send(const uint32_t targetId,
                     const char *data,
                     const int datalen,
                     const int majorTypeId,
                     const int minorTypeId,
                     const int messageId /* = 0 */,
                     const int /* messageFlags = 0 */)
  {
    assert(datalen >= 0 );
    assert(data != NULL || datalen != 0);

    int result = GIMI_ERROR;
    int msgActFlags = 0; // Not supported at this point.
    uint32_t msgUid = giveUid();

    // Send message
    BiteStream *msg = bsAlloc();

    makeMessage(msg,
                GIMI_PROTOCOL_DATA, 
                msgUid, 
                msgActFlags, 
                majorTypeId, 
                minorTypeId,
                messageId, 
                0, 
                data, 
                datalen);
  
    result = iConnector->sendDataToTarget(targetId, 
                                          (uint8_t*)msg->buf,
                                          msg->length);
    bsFree(msg);

    return result;
  }

  int GIMICore::sendToSubscribers(const char *data,
                                  const int datalen,
                                  const int majorTypeId,
                                  const int minorTypeId,
                                  const int messageId,
                                  const int /*messageFlags*/,
                                  const int maxUnsynced, /* = -1 */
                                  const int timeout /* = 500 */)
  {
    assert(datalen >= 0 );
    assert(data != NULL || datalen != 0);

    if (!iConnector->isConnected()) {
      return GIMI_NOCONNECTION;
    }
  
    t_clientIdList subscriberList;
    int result = GIMI_ERROR;
    int msgActFlags = 0;
    uint32_t msgUid = giveUid();

    if (maxUnsynced > -1) {
      msgActFlags += GIMI_ACT_REQUESTREPLY;
    }

    /*
      result = subscribers.getSubscribers(&subscriberArray, &nSubscribers,
      majorTypeId, minorTypeId, msgUid,
      maxUnsynced, timeout);
    */
    result = subscribers.getSubscribers(subscriberList, majorTypeId, minorTypeId,
                                        msgUid, maxUnsynced, timeout);
  
    int nSubscribers = subscriberList.size();

    if (result == GIMI_OK) {
      if (nSubscribers > 0) {
        BiteStream *msg = bsAlloc();
        makeMessage(msg, GIMI_PROTOCOL_DATA_SUBSCRIBED, msgUid, msgActFlags, majorTypeId,
                    minorTypeId, messageId, 0, data, datalen);
      
        result = iConnector->sendDataMulticast(subscriberList, 
                                               (uint8_t *)msg->buf,
                                               msg->length);
        bsFree(msg);
  
      } else {
        result = GIMI_NOSUBSCRIBERS;

      }
    } else {
      dPrint(ODTEST, "getSubscribers failed, likely type %d:%d was not found.",
             majorTypeId, minorTypeId);
    }

 
    return result;
  }

  int GIMICore::serviceDiscovery(t_serviceList &services,
                                 const unsigned int aTimeout_ms)
  {
    // Split timeout to SD timeout and Nameservice timeout
    const unsigned int sd_timeout = (2*aTimeout_ms)/3;
    const unsigned int ns_timeout = aTimeout_ms-sd_timeout;

    // Mark time of entry to function.
    const ownTime_ms_t begin_time = ownTime_get_ms();

    // Don't do anything if not connected.
    if (!iConnector->isConnected()) {
      dPrint(1, "We are not connected to hub!");
      return GIMI_NOCONNECTION;
    }
  
    // Clear services.
    services.clear();

    // Message unique identifier (From GIMIs internal pool)
    const uint32_t uid = giveUid();

    Lock(KSyncObjectServiceDiscoveryBundleArray);
    // Register Bundle (Add empty list to table; first check 
    // VERIFY: that the ID does not exist in the table yet!
    assert(iServiceDiscoveryBundleArray.find(uid) == iServiceDiscoveryBundleArray.end());
    iServiceDiscoveryBundleArray[uid] = TServiceListArray();
    Unlock(KSyncObjectServiceDiscoveryBundleArray);


    BiteStream *msg = bsAlloc();
  
    // Construct Probe message
    makeMessage(msg, GIMI_PROTOCOL_CONTROL_SERVICES, 
                uid, 
                GIMI_ACT_DOCOMMAND | GIMI_ACT_REQUESTREPLY, 
                0, 
                0,
                0,
                GIMI_CMD_LIST);

 
    // Now, send the probe through Server Side multicast
    unsigned int deliveredCount = 0;
    const ownTime_ms_delta_t t_left_send = ownTime_get_ms_left(begin_time, sd_timeout);
    const bool rsend = iConnector->sendDataByServerMulticast(deliveredCount,
                                                             (const uint8_t *)msg->buf, 
                                                             msg->length,
                                                             KMulticastTypeServiceDiscovery,
                                                             t_left_send);
    bsFree(msg);

    // Now check result.
    if (rsend) {
      Lock(KSyncObjectServiceDiscoveryBundleArray);

      // Now; wait until we have 'deliveredCount' of messages
      ownTime_ms_delta_t t_left;
      TServiceListBundleArray::iterator slbai = iServiceDiscoveryBundleArray.find(uid);
      TServiceListArray &sla = slbai->second;
      while(sla.size() < deliveredCount &&
            (t_left = ownTime_get_ms_left(begin_time, sd_timeout)) > 0) {
      
        //     dPrintLCGreen(ODTEST,"WAIT: %ums", t_left);
        Wait(t_left, KSyncObjectServiceDiscoveryBundleArray, KSyncObjectServiceDiscoveryBundleArray);
      }
    
      if (sla.size() == deliveredCount) {
        //dPrint(ODTEST,"ServiceDiscovery: Expected %u replies, got %u replies - all here",
        //      deliveredCount, sla.size());

      } else if (sla.size() > deliveredCount) {
        dPrint(ODTEST,"WTF?! Got %u replies while expecting %u!", 
               sla.size(), deliveredCount);
        
      } else {
        dPrint(3,"ServiceDiscovery: Expected %u replies, got %u replies - not all probes were received in timeout of %u ms",
               deliveredCount, sla.size(), sd_timeout);

      }
      
      // Now; copy the array contents.
      TServiceListArray sla_copy = sla;

      // Now, cleanup unconditionally.
      TServiceListBundleArray::iterator cleanit = iServiceDiscoveryBundleArray.find(uid);
      if (cleanit != iServiceDiscoveryBundleArray.end()) iServiceDiscoveryBundleArray.erase(cleanit);
      
      Unlock(KSyncObjectServiceDiscoveryBundleArray);
      
      ////////////// Proceed to Namequery
      // Now; Reschedule
      ownTime_ms_t ns_begin_time = ownTime_get_ms(); 
      for(EACH_IN_i(sla_copy)) {
        bool all_services_found = true;
        for(EACH_IN_j(*i)) {
          if (j->clientName.empty()) {
            t_left = ownTime_get_ms_left(ns_begin_time, ns_timeout);
            //dPrint(ODTEST,"Fetching name for client %08x (Timeout in %d ms)", j->clientId, t_left);
            int r = iConnector->GetClientNameById(j->clientName,
                                                  j->clientId,
                                                  t_left);
            if (r != GIMI_OK) {
              dPrintLCRed(ODERROR,"Failed to get name for %08x!", j->clientId);
              all_services_found = false;
              break;
            }
          }
        }
        if (all_services_found) {
          services.insert(services.end(), i->begin(), i->end());
        }
      }
      
      // For statistics.
      const unsigned int service_count = services.size();
      const unsigned int instance_count = sla_copy.size();
      const ownTime_ms_delta_t took_ms_count = ownTime_get_ms_since(begin_time);
      
      dPrint(3,"ServiceDiscovery Completed - Query took %u ms, received %u services from %u GIMI instances", 
             took_ms_count, service_count, instance_count);
    
    } else {
      dPrintLCRed(ODERROR,"sendDataByServerMulticast failed to execute in given timeout (%ums)",
                  t_left_send);
           
    }
    
    //dPrint(ODTEST,"Services size: %u", services.size());

    return GIMI_OK;
  }

  int GIMICore::serviceDiscoveryToTarget(t_serviceList &services,
                                         const unsigned int &aClientId,
                                         const unsigned int &aTimeout_ms)
  {
#warning FIX THIS TOO
    // Mark time of entry to function.
    const ownTime_ms_t begin_time = ownTime_get_ms();

    // Don't do anything if not connected.
    if (!iConnector->isConnected()) {
      dPrint(1, "We are not connected to hub!");
      return GIMI_NOCONNECTION;
    }
  
    // Clear services.
    services.clear();

    // Message unique identifier (From GIMIs internal pool)
    const uint32_t uid = giveUid();

    Lock(KSyncObjectServiceDiscoveryBundleArray);
    // Register Bundle (Add empty list to table; first check 
    // VERIFY: that the ID does not exist in the table yet!
    assert(iServiceDiscoveryBundleArray.find(uid) == iServiceDiscoveryBundleArray.end());
    iServiceDiscoveryBundleArray[uid] = TServiceListArray();
    Unlock(KSyncObjectServiceDiscoveryBundleArray);


    BiteStream *msg = bsAlloc();
    
    // Construct Probe message
    makeMessage(msg, GIMI_PROTOCOL_CONTROL_SERVICES, 
                uid, 
                GIMI_ACT_DOCOMMAND | GIMI_ACT_REQUESTREPLY, 
                0, 
                0,
                0,
                GIMI_CMD_LIST);


    // Now, send the probe through Server Side multicast
    unsigned int deliveredCount = 1; // This is SINGLE SEND!
    const int rsend = iConnector->sendDataToTarget(aClientId, 
                                                   (uint8_t *)msg->buf,
                                                   msg->length);
    bsFree(msg);
    Lock(KSyncObjectServiceDiscoveryBundleArray);

    // Now check result.
    if (rsend == GIMI_OK) {
    
      // Now; wait until we have 'deliveredCount' of messages
      ownTime_ms_delta_t t_left;
      TServiceListBundleArray::iterator slbai = iServiceDiscoveryBundleArray.find(uid);
      TServiceListArray &sla = slbai->second;
      while(sla.size() < deliveredCount &&
            (t_left = ownTime_get_ms_left(begin_time, aTimeout_ms)) > 0) {
      
        //     dPrintLCGreen(ODTEST,"WAIT: %ums", t_left);
        Wait(t_left, KSyncObjectServiceDiscoveryBundleArray, KSyncObjectServiceDiscoveryBundleArray);
      }
    
      if (sla.size() == deliveredCount) {
        //dPrint(ODTEST,"ServiceDiscovery: Expected %u replies, got %u replies - all here",
        //      deliveredCount, sla.size());

      } else {
        dPrint(ODTEST,"ServiceDiscovery: Expected %u replies, got %u replies - not all probes were received in timeout of %u ms",
               deliveredCount, sla.size(), aTimeout_ms);

      }
    
      // Now; Copy the results to caller.
      for(EACH_IN_i(sla)) {
        services.insert(services.end(), i->begin(), i->end());
      }
    
      // For statistics.
      const int service_count = services.size();
      const int instance_count = sla.size();
      const ownTime_ms_delta_t took_ms_count = ownTime_get_ms_since(begin_time);
    
      dPrint(3,"ServiceDiscoveryToTarget Completed - Query took %u ms, received %d services from %d GIMI instance", 
             took_ms_count, service_count, instance_count);
      assert(instance_count == 1 || instance_count == 0);
    
    } else {
      dPrint(ODTEST,"sendDataToTarget failed to execute!");
    
    }

    // Cleanup unconditionally.
    iServiceDiscoveryBundleArray.erase(iServiceDiscoveryBundleArray.find(uid));

    Unlock(KSyncObjectServiceDiscoveryBundleArray);

    return GIMI_OK;
  }

  int GIMICore::ping(const int clientId,
                     const int timeoutms)
  {
    dPrint(16, "Called");
    // Don't do anything if not connected.
    if (!isConnected()) {
      dPrint(1, "We are not connected to hub!");
      return GIMI_NOCONNECTION;
    }
  
    if (timeoutms < 0) {
      dPrint(5, "Timeout cannot be negative!");
      return GIMI_ERROR;
    }

    int result, replyResult = GIMI_REPLYID_WASNOTREPLIED;
    int msgActFlags = GIMI_ACT_DOCOMMAND + GIMI_ACT_REQUESTREPLY;
    uint32_t uid = giveUid();
    gim::time timeoutTime, starttime, timediff;

    starttime.setToCurrent();
    timeoutTime.setToCurrentPlus(timeoutms);
    replyHandler.addUnreplied(uid, timeoutTime); // Add to reply wait-list.
  
    BiteStream *msg = bsAlloc();

    // Send the ping-message itself.
    makeMessage(msg, GIMI_PROTOCOL_CONTROL_GENERIC, uid, msgActFlags, 0, 0,
                0, GIMI_CMD_PING);
    result = iConnector->sendDataToTarget(clientId, 
                                          (uint8_t *)msg->buf,
                                          msg->length);
    // Unlock OutStream
    bsFree(msg);

    // Check if send succeeded.
    if(result != GIMI_OK) {
      dPrint(2, "Send failed with return value: %d", result);
      return GIMI_ERROR;
    }
  
    dPrint(18, "Ping sent to 0x%08x", clientId);

    Lock(KSyncObjectReplyQueue);
  
    while (replyResult == GIMI_REPLYID_WASNOTREPLIED && activeStatus.isActive())
      {
        Wait(KSyncObjectReplyQueue, KSyncObjectReplyQueue, timeoutms);

        replyResult = replyHandler.checkForReply(uid);

        switch (replyResult) {
        case GIMI_REPLYID_WASNOTREPLIED:
          dPrint(30, "WasNotReplied");
          break;
        case GIMI_REPLYID_WASREPLIED:
          dPrint(30, "WasReplied");
          result = GIMI_OK;
          break;
        case GIMI_REPLYID_TIMEOUTED:
          dPrint(30, "Timeouted");
          result = GIMI_TIMEOUT;
          break;
        case GIMI_REPLYID_NOTFOUND:
          dPrint(6, "UniqueID not found in replyhandler. Internal error.", replyResult);
          assert(false);
          break;
        default:
          dPrint(6, "Unknown replyResult %d. Internal error.", replyResult);
          assert(false);
          break;
        }
      }
    timediff.setToCurrent();
    Unlock(KSyncObjectReplyQueue);
  
    if (!activeStatus.isActive()) {
      dPrint(18, "Waiting for reply interrupted, GIMI no longer active.");
      result = GIMI_INTERRUPT;
    }
  
    if (result != GIMI_OK) {
      return result;
    }
  
    timediff -= starttime;
    return timediff.getTimeMs();
  }

  int GIMICore::setTargetDebugLevel(const int clientId,
                                    const int debugLevel)
  {
    dPrint(10, "Called");
    // Don't do anything if not connected.
    if (!isConnected()) {
      dPrint(1, "We are not connected to hub!");
      return GIMI_NOCONNECTION;
    }

    int result = GIMI_ERROR;
    int msgActFlag = GIMI_ACT_DOCOMMAND;
    uint32_t msgUid = giveUid();

    BiteStream *msg = bsAlloc();
    makeMessage(msg,GIMI_PROTOCOL_CONTROL_GENERIC, msgUid, msgActFlag, debugLevel, 0,
                0, GIMI_CMD_SETDEBUGLEVEL);
    result = iConnector->sendDataToTarget(clientId, 
                                          (uint8_t *)msg->buf,
                                          msg->length);
    bsFree(msg);
  
    if(result != GIMI_OK) {
      dPrint(2, "Send failed with return value: %d", result);
      return GIMI_ERROR;
    }

    dPrint(15, "Setdebuglevel to %d successfully sent to 0x%08x", debugLevel,
           clientId);
    return GIMI_OK;
  }

  void GIMICore::handleGimiMessage(const uint8_t *messageData,
                                   const unsigned int messageDataLength,
                                   const uint32_t senderId)
  {
    //  dPrint(ODTEST, "Called with %d bytes of payload from 0x%08x", messageDataLength, senderId);

    bool result = false;
    int msgDataType, msgActFlags, majorTypeId, minorTypeId;
    uint32_t msgUid;
  
    BiteStream *inStream;
    bsTag *tag;
    GIMIMessage *msg;
  
    // First decode the message
    inStream = bsDecode((char *)(messageData)); //TODO bsDecode should be changed to take const char*
    if (inStream == NULL) {
      dPrint(ODTEST, "Bitestream could not decode received data.");
      return;
    }

    if (inStream->numtags < GIMI_MSG_TAGN_MIN || inStream->numtags > GIMI_MSG_TAGN_MAX) {
      dPrint(ODTEST, "Invalid number of tags.");
      bsFree(inStream);
      return;
    }

    // Data was BiteStream type, continue handling.

    // Determine if there is a tag and if it is the right type.
    tag = bsNextTag(inStream);
    if (tag == NULL || tag->tag != GIM_PROTOCOL_GIMI_MESSAGE) {
      dPrint(ODTEST, "First tag was null or wrong type, ignore message");
      bsFree(inStream);
      return;
    }
    msgDataType = tag->value;

    // Get unique id.
    tag = bsNextTag(inStream);
    if (tag == NULL || tag->tag != GIMITAG_UID) {
      dPrint(ODTEST, "Second tag was null or wrong type, ignore message");
      bsFree(inStream);
      return;
    }
    msgUid = tag->value; //TODO FIXME FIXME Is this direct copy or conversion, what happens with uint->int->uint?

    // Get actionflags.
    tag = bsNextTag(inStream);
    if (tag == NULL || tag->tag != GIMITAG_ACTION) {
      dPrint(ODTEST, "Third tag was null or wrong type, ignore message");
      bsFree(inStream);
      return;
    }
    msgActFlags = tag->value;
  
    // Get majortype
    tag = bsNextTag(inStream);
    if (tag == NULL || tag->tag != GIMITAG_MAJORTYPE) {
      dPrint(ODTEST, "Fourth tag was null or wrong type, ignore message");
      bsFree(inStream);
      return;
    }
    majorTypeId = tag->value;

    // Get minortype
    tag = bsNextTag(inStream);
    if (tag == NULL || tag->tag != GIMITAG_MINORTYPE) {
      dPrint(ODTEST, "Fifth tag was null or wrong type, ignore message");
      bsFree(inStream);
      return;
    }
    minorTypeId = tag->value;
  
    // Move to first actual message tag.
    tag = bsNextTag(inStream);
  
    msg = new GIMIMessage();

    // Set sender
    msg->senderId = senderId;
  
    // Assign types.
    msg->majorTypeId = majorTypeId;
    msg->minorTypeId = minorTypeId;

    // Assign timestamp.
    msg->arrivalTime.setToCurrent();
    
    // Assign stuff from nif.
    msg->statusMessage = false;
  
    //TODO
    msg->messageFlags = 0x0;
    /*
      msg->messageFlags = p->flags;
    */
  
    switch(msgDataType) {
    case GIMI_PROTOCOL_DATA:
    case GIMI_PROTOCOL_DATA_SUBSCRIBED:
      if (defaultBufferSize < 1) { // Ignore all received data.
        dPrint(ODTEST, "Not receiving data.");
        result = false;
        break;
      }
      result = processDataMessage(&msg, inStream, tag, msgUid, msgActFlags, msgDataType);
      if (!result) { // Message was not added to a queue.
        dPrint(ODTEST, "Datamessage was invalid.");
      }
      break;
    case GIMI_PROTOCOL_CONTROL_DATA:
    case GIMI_PROTOCOL_CONTROL_DATA_SUBSCRIBED:
    case GIMI_PROTOCOL_CONTROL_GENERIC:
    case GIMI_PROTOCOL_CONTROL_SERVICES:
      result = processControlMessage(&msg, inStream, tag, msgUid, msgActFlags, msgDataType);
      if (!result)
        dPrint(ODTEST, "Controlmessage invalid or command could not be performed.");
      break;
    default:
      dPrint(ODTEST, "PROTOCOL had invalid value %d", tag->value);
      result = false;
      break;
    }
  
    // Free memory reserved by BiteStream
    bsFree(inStream);

    // If messagepointer exists (has not been added to a buffer) free it.
    if (msg != NULL) {
      delete msg;
    }
  
    return;
  }

  void GIMICore::stop()
  {
    dPrint(16, "Called");
  
    // Tells methods to stop listening loops.
    dPrint(16, "Set ActiveStatus to false");
    activeStatus.deactivate();

    // Tell connection to stop dataReceiverThread
    //iConnector->activeStatus.deactivate();
    dPrint(16, "Tell connection to stop.");
    iConnector->stop();
  
    // Wake up methods in case they are waiting for a condition.
    messageBuffers.broadcastCond();
    ownCond_Broadcast(replyHandler.checkReplyCondHandle);
    dPrint(16, "Buffers and replyhandler have been sent broadcast.");
  }

  uint32_t GIMICore::giveUid()
  {
    Lock(KSyncObjectUniqueId);
    const uint32_t unique_id = ++iUniqueIdentifier;
    Unlock(KSyncObjectUniqueId);

    return unique_id;
  }

  int GIMICore::subscribeCore(const int clientId,
                              const int majorServiceId,
                              const int minorServiceId,
                              bool doUnsubscribe /* = false */,
                              const int timeoutms /* = 2500 */)
  {
    // Don't do anything if not connected.
    if (!iConnector->isConnected()) {
      dPrint(1, "We are not connected to hub!");
      return GIMI_NOCONNECTION;
    }
  
    if (timeoutms < 0) {
      dPrint(2, "Timeout cannot be negative!");
      return GIMI_ERROR;
    }
  
    int result = GIMI_ERROR, replyResult = GIMI_REPLYID_WASNOTREPLIED;
    int msgActFlag = GIMI_ACT_DOCOMMAND;
    uint32_t uid = giveUid();
    int datalength = 0;
    char *data = NULL;
    gim::time timeoutTime;
    
    if (timeoutms > 0) {
      timeoutTime.setToCurrentPlus(timeoutms);
      replyHandler.addUnreplied(uid, timeoutTime); // Add to reply wait-list.
      msgActFlag += GIMI_ACT_REQUESTREPLY;
    }
  
    BiteStream *msg = bsAlloc();
    // Send subscribe-message
    if (doUnsubscribe) {
      makeMessage(msg,GIMI_PROTOCOL_CONTROL_SERVICES, uid, msgActFlag,
                  majorServiceId, minorServiceId, 0, GIMI_CMD_UNSUBSCRIBE);
 
    } else {
      makeMessage(msg,GIMI_PROTOCOL_CONTROL_SERVICES, uid, msgActFlag,
                  majorServiceId, minorServiceId, 0, GIMI_CMD_SUBSCRIBE);
  
    }
    result = iConnector->sendDataToTarget(clientId, 
                                          (uint8_t *)msg->buf,
                                          msg->length);
    bsFree(msg);


    // Check if send succeeded.
    if(result != 0) {
      dPrint(2, "Send failed with return value: %d", result);
      return GIMI_ERROR;
    }
  
    if (doUnsubscribe) {
      dPrint(18, "Unsubscribe to %d:%d successfully sent to 0x%08x", majorServiceId,
             minorServiceId, clientId);

    } else {
      dPrint(18, "Subscribe to %d:%d successfully sent to 0x%08x", majorServiceId,
             minorServiceId, clientId);

    }
  
    if (timeoutms > 0) {
  
      Lock(KSyncObjectReplyQueue);

  
      // Process answer Queue
      const ownTime_ms_t loopbegin = ownTime_get_ms();
      ownTime_ms_delta_t tleft = timeoutms;
      do {
        replyResult = replyHandler.checkForReply(uid, &data, &datalength);
        tleft = ownTime_get_ms_left(timeoutms, loopbegin);

        // Now, check result; if not replied; WAIT(), otherwise break;
        if (replyResult == GIMI_REPLYID_WASNOTREPLIED) {
          Wait(tleft, KSyncObjectReplyQueue, KSyncObjectReplyQueue);
        } else {
          break;
        }
      
      } while(tleft > 0 && 
              activeStatus.isActive());

      Unlock(KSyncObjectReplyQueue);


      // After loop terminated, act upon value
      switch (replyResult) {
      case GIMI_REPLYID_WASNOTREPLIED:
        //      dPrint(ODTEST, "WasNotReplied");
        break;
      case GIMI_REPLYID_WASREPLIED:
        //dPrint(ODTEST, "WasReplied");
        result = GIMI_OK;
        break;
      case GIMI_REPLYID_TIMEOUTED:
        //dPrint(ODTEST, "Timeouted");
        result = GIMI_TIMEOUT;
        break;
      case GIMI_REPLYID_NOTFOUND:
        dPrint(ODTEST, "UniqueID not found in replyhandler. Internal error.", replyResult);
        abort();
        break;
      default:
        dPrint(ODTEST, "Unknown replyResult %d. Internal error.", replyResult);
        abort();
        break;
      }
    } else {
      result = GIMI_TIMEOUT;
    }
  
    // Process active status flag
    if (!activeStatus.isActive()) {
      result = GIMI_INTERRUPT;
    }

    // If FINAL result was GIMI_OK, process the replydata.
    if (result == GIMI_OK) {
      int replyValue = GIMI_ERROR;
      if (!processReplyData(&replyValue, data, datalength)) {
        if (doUnsubscribe) {
          dPrint(ODTEST, "Invalid reply for unsubscription request from 0x%08x", clientId);

        } else {
          dPrint(ODTEST, "Invalid reply for subscription request from 0x%08x", clientId);

        }
      }
      result = replyValue;
    }
  
    // Cleanup data from replyHandler.
    if (data != NULL) {
      delete[] data;
    }
  
    return result;
  }

  int GIMICore::sendServiceList(const int targetId,
                                const uint32_t msgUid)
  {
    t_serviceList serviceList;
    t_serviceListConstIterator slistiter;
  
    //int provided = 
    subscribers.listServices(serviceList);
    //dPrint(ODTEST, "Got %d provided types.", provided);
    
    //int accepted = 
    messageFilter.listWantedTypes(serviceList);
    //dPrint(ODTEST, "Got %d accepted types.", accepted);
    
    BiteStream *serviceData = bsAlloc();
  
    // Create BiteStream containing list of services with descriptions.
    for (slistiter = serviceList.begin();
         slistiter != serviceList.end();
         ++slistiter)
      {
        /*dPrint(ODTEST, "%s %d:%d %s @ %s",
          slistiter->getShortServiceTypeString().c_str(),
          slistiter->majorServiceId,
          slistiter->minorServiceId,
          (slistiter->serviceDescription).c_str(),
          (slistiter->serviceLocation).c_str());*/
        bsAddInt(serviceData, GIMITAG_MAJORTYPE, slistiter->majorServiceId);
        bsAddInt(serviceData, GIMITAG_MINORTYPE, slistiter->minorServiceId);
        bsAddInt(serviceData, GIMITAG_SERVICETYPE, slistiter->serviceType);
    
        if (!(slistiter->serviceDescription).empty()) { // Add description if exists.
          bsAddStr(serviceData, GIMITAG_SERVICEDESCRIPTION, (slistiter->serviceDescription).c_str());
        }
        if (!(slistiter->serviceLocation).empty()) { // Add location if exists.
          bsAddStr(serviceData, GIMITAG_SERVICELOCATION, (slistiter->serviceLocation).c_str());
        }
        // Add version information if this is the client description.
        if (slistiter->majorServiceId == GIMI_PROTOCOL_CLIENT_DESCRIPTION &&
            slistiter->minorServiceId == 0 &&
            slistiter->serviceType == GIMI_SERVICETYPE_DESCRIPTION)
          {
            //dPrint(ODTEST, "Add version information: GIMI %d build %d", GIMI_VERSION,
            //       BUILD_NUMBER_GIMI);
            bsAddInt(serviceData, GIMITAG_VERSION, GIMI_VERSION);
            bsAddInt(serviceData, GIMITAG_BUILD, BUILD_NUMBER_GIMI);
      
            // Add client version-information if it exists.
            if (clientVersion.getVersion() != 0) {
              //dPrint(ODTEST, "Add client version information: %d build %d",
              //       clientVersion.getVersion(), clientVersion.getBuild());
              bsAddInt(serviceData, GIMITAG_CLIENTVERSION, clientVersion.getVersion());
              bsAddInt(serviceData, GIMITAG_CLIENTBUILD, clientVersion.getBuild());
            }
          }
      }
  
    bsFinalize(serviceData);
  
  
    // Make message.
    BiteStream *msg = bsAlloc();
    makeMessage(msg, GIMI_PROTOCOL_CONTROL_SERVICES, msgUid, 0, 0, 0, 0, GIMI_CMD_LIST,
                serviceData->buf, serviceData->length);
  
    // Send it.
    int result = iConnector->sendDataToTarget(targetId, 
                                              (uint8_t *)msg->buf,
                                              msg->length);
  
    bsFree(serviceData);
    bsFree(msg);
  
    if(result != GIMI_OK) {
      dPrint(ODTEST, "Could not send servicelist to 0x%08x with uid %d.", targetId, msgUid);
      //linkValid = false;
      return GIMI_NOCONNECTION;
    }
    //  dPrint(ODTEST, "Servicelist successfully sent to 0x%08x with uid %d.", targetId, msgUid);
  
    return GIMI_OK;
  }

  int GIMICore::processServiceDiscoveryData(t_serviceList &servicelist,
                                            const int clientId,
                                            const std::string clientName,
                                            const char *data,
                                            const int datalength)
  {
    BiteStream *serviceData;
  
    if (data != NULL && datalength > 0) { // Make sure there is some data.
      serviceData = bsDecode(const_cast<char *>(data));
      bsTag *tag;
      int majorType, minorType, serviceType, version = 0, build = 0;
      int cVersion = 0, cBuild = 0;
      std::string serviceDescription, serviceLocation;
      if (serviceData != NULL) { // Check if data was BiteStream.
        bool tagLoop = true;
        tag = bsNextTag(serviceData);
        while (tagLoop) {
          // Loops until tags run out or GIMITAG_MAJORTYPE is found.
          // This way unknown tags at the end of listing won't break parsing.
          while (tag != NULL && tag->tag != GIMITAG_MAJORTYPE) {
            tag = bsNextTag(serviceData);
          }
          if (tag != NULL && tag->tag == GIMITAG_MAJORTYPE) {
            //          dPrint(30, "Found majortype.");
            majorType = tag->value;
            tag = bsNextTag(serviceData);
            if (tag != NULL && tag->tag == GIMITAG_MINORTYPE) {
              //dPrint(30, "Found minortype.");
              minorType = tag->value;
              tag = bsNextTag(serviceData);
              if (tag != NULL && tag->tag == GIMITAG_SERVICETYPE) {
                //dPrint(30, "Found type of service.");
                serviceType = tag->value;
                tag = bsNextTag(serviceData);
                if (tag != NULL && tag->tag == GIMITAG_SERVICEDESCRIPTION) {
                  //dPrint(30, "Found description.");
                  serviceDescription.assign(tag->string, tag->length);
                  tag = bsNextTag(serviceData);
                } else { // Clear description if it was not included.
                  //dPrint(30, "No description.");
                  serviceDescription.clear();
                }
                if (tag != NULL && tag->tag == GIMITAG_SERVICELOCATION) {
                  //dPrint(30, "Found location.");
                  serviceLocation.assign(tag->string, tag->length);
                  tag = bsNextTag(serviceData);
                } else { // Clear description if it was not included.
                  //dPrint(30, "No location.");
                  serviceLocation.clear();
                }
                // If this is a client description, find out it's version.
                if (majorType == GIMI_PROTOCOL_CLIENT_DESCRIPTION &&
                    minorType == 0 &&
                    serviceType == GIMI_SERVICETYPE_DESCRIPTION)
                  {
                    if (tag != NULL && tag->tag == GIMITAG_VERSION) {
                      //dPrint(30, "Found version.");
                      version = tag->value;
                      tag = bsNextTag(serviceData);
                    }
                    if (tag != NULL && tag->tag == GIMITAG_BUILD) {
                      //dPrint(30, "Found build.");
                      build = tag->value;
                      tag = bsNextTag(serviceData);
                    }
                    if (tag != NULL && tag->tag == GIMITAG_CLIENTVERSION) {
                      //dPrint(30, "Found client version.");
                      cVersion = tag->value;
                      tag = bsNextTag(serviceData);
                    }
                    if (tag != NULL && tag->tag == GIMITAG_CLIENTBUILD) {
                      //dPrint(30, "Found client build.");
                      cBuild = tag->value;
                      tag = bsNextTag(serviceData);
                    }
                  } else {
                  version = 0;
                  build = 0;
                  cVersion = 0;
                  cBuild = 0;
                }
              
                // Add to servicelist.
                servicelist.push_back(GIMIService(clientId, clientName,
                                                  serviceType,
                                                  majorType, minorType,
                                                  serviceDescription,
                                                  serviceLocation,
                                                  version, build, cVersion,
                                                  cBuild));
              } else {
                tagLoop = false;

              }
            } else {
              tagLoop = false;

            }
          } else {
            tagLoop = false;

          }
        }
      
        // Cleanup decoded BiteStream
        bsFree(serviceData);
      
        return GIMI_OK;
      }
    } else {
      dPrint(18, "Given datapointer was null or datalength was < 1");
      return GIMI_ERROR; 
    }
    return GIMI_ERROR;
  }

  int GIMICore::sendReplyMessage(const int targetId,
                                 const int msgDataType,
                                 const uint32_t msgUid,
                                 const int msgActFlags,
                                 const int majorTypeId,
                                 const int minorTypeId,
                                 const int msgCommand,
                                 const int replyValue /* = GIMI_INVALIDREPLYVALUE */)
  {
    //dPrint(ODTEST, "Called with: target 0x%08x datatype %d uid %d flags %d "
    //       "major %d minor %d command %d replyValue %d", targetId, msgDataType,
    //      msgUid, msgActFlags, majorTypeId, minorTypeId, msgCommand, replyValue);
    int result;

    BiteStream *msg = bsAlloc();

    // Don't do anything if not connected.
    if (!iConnector->isConnected()) {
      return GIMI_NOCONNECTION;
    }
  
    if (replyValue != GIMI_INVALIDREPLYVALUE) { // If replyvalue is defined
      BiteStream *replyData = bsAlloc();
      bsAddInt(replyData, GIMITAG_REPLY, replyValue);
      bsFinalize(replyData);
    
      makeMessage(msg, msgDataType, msgUid, 0, majorTypeId, minorTypeId, 0, msgCommand,
                  replyData->buf, replyData->length);
      result = iConnector->sendDataToTarget(targetId, 
                                            (uint8_t *)msg->buf,
                                            msg->length);
                                         
      bsFree(replyData);

    } else {

      makeMessage(msg, msgDataType, msgUid, 0, majorTypeId, minorTypeId, 0, msgCommand);
      result = iConnector->sendDataToTarget(targetId,
                                            (uint8_t*)msg->buf,
                                            msg->length);
    }

    bsFree(msg);
  
    if(result != GIMI_OK) {
      dPrint(ODTEST, "Could not send reply to sender 0x%08x about %d.", targetId,
             msgUid);
      //linkValid = false;
      return GIMI_NOCONNECTION;
    }
    //  dPrint(ODTEST, "Reply successfully sent to 0x%08x about %d.", targetId, msgUid);
    return GIMI_OK;
  }

  void GIMICore::upkeepKeepAlive()
  {
    t_serviceNumberList services;
    t_serviceNumberListConstIterator iter;
    int n = 0, m = 0;
  
    //ownMutex_Lock(subscriberMutex);
    n = subscribers.listServicesForKeepAlive(services);
    //ownMutex_Unlock(subscriberMutex);
  
    //dPrint(16, "%d services need to send a keepalive.", n);
  
    for (iter = services.begin();
         iter != services.end();
         ++iter)
      {
        if (sendKeepAliveToSubscribers(iter->first, iter->second) != GIMI_OK) {
          dPrint(2, "Sending keepalive to subscribers of %d:%d failed.",
                 iter->first, iter->second);
          ++m;
        }
      }
  
    if (m > 0) {
      dPrint(1, "Failed to send keepalive to %d services of total %d.", m, n); 
    }
  }

  int GIMICore::sendKeepAliveToSubscribers(const int majorTypeId,
                                           const int minorTypeId)
  {
    // Don't do anything if not connected.
    if (!iConnector->isConnected()) {
      dPrint(1, "We are not connected to hub!");
      return GIMI_NOCONNECTION;
    }
  
    t_clientIdList subscriberList;
    int nSubscribers = 0, result = GIMI_ERROR;
    uint32_t msgUid = giveUid();
  
    result = subscribers.getSubscribers(subscriberList, majorTypeId, minorTypeId, 0);
    nSubscribers = subscriberList.size();
  
    dPrint(16, "Got %d subscribers for service %d:%d.", nSubscribers,
           majorTypeId, minorTypeId);
  
    if (result == GIMI_OK) {
      if (nSubscribers > 0) {
        dPrint(14, "Sending keepalive to subscribers.");
        BiteStream *msg = bsAlloc();
        makeMessage(msg,GIMI_PROTOCOL_CONTROL_DATA_SUBSCRIBED, msgUid, 0,
                    majorTypeId, minorTypeId, 0, GIMI_CMD_KEEPALIVE);
        result = iConnector->sendDataMulticast(subscriberList,
                                               (uint8_t *)msg->buf,
                                               msg->length);
        bsFree(msg);
        if (result != GIMI_OK) {
          dPrint(2, "Sending failed.");
        } else {
          dPrint(14, "Sending succeeded.");
        }
      } else {
        dPrint(14, "Got no subscribers.");
        result = GIMI_NOSUBSCRIBERS;
      }
    } else {
      dPrint(14, "getSubscribers failed, likely type %d:%d was not found.",
             majorTypeId, minorTypeId);
    }
  
    return result;
  }


  bool GIMICore::processReplyData(int *replyValue,
                                  const char *data,
                                  const int datalength)
  {
    BiteStream *replyData;
  
    if (data != NULL && datalength > 0) { // Make sure there is some data.
      replyData = bsDecode(const_cast<char *>(data));
      bsTag *tag;
      if (replyData != NULL) { // Check if data was BiteStream.
        tag = bsNextTag(replyData);
        if (tag != NULL && tag->tag == GIMITAG_REPLY) {
          dPrint(30, "Found replyValue.");
          *replyValue = tag->value;
        
          // Cleanup decoded BiteStream
          bsFree(replyData);
      
          return true;
        }
      
        // Cleanup decoded BiteStream
        bsFree(replyData);
      }
    } else {
      dPrint(18, "Given datapointer was null or datalength was < 1");
      return false; 
    }
    return false;
  }

  bool GIMICore::makeMessage(BiteStream *outStream,
                             const int msgDataType,
                             const uint32_t msgUid,
                             const int msgActFlags,
                             const int majorTypeId,
                             const int minorTypeId,
                             const int msgId,
                             const int msgCommand,
                             const char* data /* = NULL */,
                             const int datalength /* = 0 */)
  {
    makeBaseMessage(outStream,
                    msgDataType, msgUid, msgActFlags, majorTypeId,
                    minorTypeId);

    if (msgDataType == GIMI_PROTOCOL_DATA ||
        msgDataType == GIMI_PROTOCOL_DATA_SUBSCRIBED)
      {
        bsAddInt(outStream, GIMITAG_MSGID, msgId); // Add messageid
      } else { // Control data
      bsAddInt(outStream, GIMITAG_COMMAND, msgCommand); // Add command-tag
    }
  
    if (data != NULL && datalength > 0) { // Add data-tag if specified.
      bsAddBin(outStream, GIMITAG_DATA, data, datalength);
    }
  
    // Finalize stream
    bsFinalize(outStream);

    return true;
  }

  void GIMICore::makeBaseMessage(BiteStream *outStream,
                                 const int msgDataType,
                                 const uint32_t msgUid,
                                 const int msgActFlags,
                                 const int majorTypeId,
                                 const int minorTypeId)
  {
    bsClear(outStream);

    // Add GIMI-protocol tag
    bsAddInt(outStream, GIM_PROTOCOL_GIMI_MESSAGE, msgDataType);

    // Add unique id.
    bsAddInt(outStream, GIMITAG_UID, msgUid);

    // Add action flags.
    bsAddInt(outStream, GIMITAG_ACTION, msgActFlags);

    // Add tags for types
    bsAddInt(outStream, GIMITAG_MAJORTYPE, majorTypeId);
    bsAddInt(outStream, GIMITAG_MINORTYPE, minorTypeId);

    return;
  }

  int GIMICore::upkeepThread(void *p)
  {
    // Block all signals to this thread.
#ifndef WINDOWS_OS
    sigset_t fullmask;
    sigfillset(&fullmask);
    pthread_sigmask(SIG_BLOCK, &fullmask, NULL);
#endif
    
    dPrint(7, "UpkeepThread start");
    
    // Get GIMICore pointer
    GIMICore *t = reinterpret_cast<GIMICore *>(p);

    gim::time lastrun, current, interval(GIMI_UPKEEPINTERVAL), difference;
    gim::time redirLastrun, redirInterval(GIMI_RECONNECTINTERVAL);

    do {
      current.setToCurrent();
      difference = current - lastrun;
      if (difference > interval) { // Run maintenance every ~GIMI_UPKEEPINTERVAL ms

        if (t->iConnector->reconnectedStatus.isActive()) {
          difference = current - redirLastrun;
          if (difference > redirInterval) {
            int n = t->messageBuffers.renewRedirectionSenderIds();
            redirLastrun.setToCurrent();
            if (n < 0) {
              dPrint(2, "Error occurred while trying to update redirection senderId:s");
              t->iConnector->reconnectedStatus.deactivate();
            } else if (n == 0) {
              dPrint(35, "Redirection senderId:s succesfully updated.");
              t->iConnector->reconnectedStatus.deactivate();
            } else {
              dPrint(16, "Tried to update redirection senderId:s, did not find "
                     "%d names. Will try again in 5 seconds.", n);
            }
          }
        }

        //// Run timeout check for replyhandler.
        t->Lock(KSyncObjectReplyQueue);
        t->replyHandler.doTimeouts();
        t->Unlock(KSyncObjectReplyQueue);

        //// Run timeout check for subscriptions.
        t->subscriptions.doResubscriptions(t);
      
        //// Check if any services want to send keepalive.
        t->upkeepKeepAlive();

        //// Run Name Re-Registration
        t->iConnector->NameRegistrationRefresh(t->iDefaultConnectorTimeout_ms);


        lastrun.setToCurrent();
        //dPrint(ODTEST, "Upkeep tasks took %d milliseconds.", (lastrun - current).getTimeMs());
      } else { // Sleep until next maintenance time.
      

        ownSleep_ms((interval - difference).getTimeMs());
      }
    } while (t->activeStatus.isActive());

    dPrint(7, "UpkeepThread end.");

    return 0;
  }

  bool GIMICore::processDataMessage(GIMIMessage **msg,
                                    BiteStream *stream,
                                    bsTag *tag,
                                    const uint32_t msgUid,
                                    const int msgActFlags,
                                    const int msgDataType)
  {
    if (msg == NULL || stream == NULL || tag == NULL || *msg == NULL) {
      return false;
    }
  
    if (stream->numtags < GIMI_MSG_TAGN_DATA_MIN ||
        stream->numtags > GIMI_MSG_TAGN_DATA_MAX )
      {
        dPrint(ODERROR, "Datatype was data, but message had %d tags!", stream->numtags);
        return false;
      }

    // Do filtering.
    //ownMutex_Lock(filterMutex);
    if (!messageFilter.isWantedType((*msg)->majorTypeId,(*msg)->minorTypeId)) {
      // Not a wanted type
      dPrint(10, "Messagetype (%d:%d) from 0x%08x was not of wanted type.",
             (*msg)->majorTypeId, (*msg)->minorTypeId, (*msg)->senderId);
      //ownMutex_Unlock(filterMutex);
      return false;
    }
    if (!messageFilter.isWantedSender((*msg)->senderId)) {
      // Not a wanted sender
      dPrint(10, "Messagetype (%d:%d) from 0x%08x was not from wanted sender.",
             (*msg)->majorTypeId, (*msg)->minorTypeId, (*msg)->senderId);
      //ownMutex_Unlock(filterMutex);
      return false;
    }
    //ownMutex_Unlock(filterMutex);
  
    if (tag->tag != GIMITAG_MSGID) {
      dPrint(15, "First msg-tag was not major-type");
      return false;
    }
    (*msg)->messageId = tag->value;

    // Copy data if there was any.
    tag = bsNextTag(stream);
    if (tag != NULL && tag->tag == GIMITAG_DATA) {
      if (tag->length > 0) {
        (*msg)->datalength = tag->length;
        (*msg)->data = new char[tag->length];
        memcpy((*msg)->data, tag->data, tag->length);
        dPrint(18, "Data length is %d", (*msg)->datalength);
      } else {
        (*msg)->datalength = 0;
        (*msg)->data = NULL;
        dPrint(6, "No data in datatag.");
      }
    } else {
      dPrint(15, "Second tag was not data-tag.");
    }
  
    //// Actions based on received data ////
    switch (msgDataType) {
    case GIMI_PROTOCOL_DATA_SUBSCRIBED:
      {
        // Refresh subscription updatetime.
        subscriptions.updateSubscriptionTime((*msg)->senderId, (*msg)->majorTypeId,
                                             (*msg)->minorTypeId);
      
        if (msgActFlags & GIMI_ACT_REQUESTREPLY) { // Reply requested.
          // Send a reply.
          dPrint(20, "Sending an ACK-reply.");
          if (sendReplyMessage((*msg)->senderId, GIMI_PROTOCOL_CONTROL_DATA_SUBSCRIBED,
                               msgUid, 0, (*msg)->majorTypeId, (*msg)->minorTypeId,
                               GIMI_CMD_PING) == GIMI_OK) {
            dPrint(20, "Message %d received reply sent to 0x%08x", msgUid,
                   (*msg)->senderId);
          } else {
            dPrint(20, "Message %d received reply failed to send to 0x%08x", msgUid,
                   (*msg)->senderId);
          }
        }
        break;
      }
    }
    ////////////////////////////////////////
  
    // Print.
    dPrint(25, "Added to buffer, datamessage type %d:%d uid %u, data %d bytes",
           (*msg)->majorTypeId, (*msg)->minorTypeId, msgUid, (*msg)->datalength);

    // Insert message to buffer.
    messageBuffers.pushMessage(*msg, (*msg)->majorTypeId, (*msg)->minorTypeId);

    // Set messagepointer to NULL so it cannot be used anymore.
    *msg = NULL;

    // Notify receivers to check queues.
    messageBuffers.broadcastCond();
  
    return true;
  }

  bool GIMICore::processControlMessage(GIMIMessage **msg,
                                       BiteStream *stream,
                                       bsTag *tag,
                                       const uint32_t msgUid,
                                       const int msgActFlags,
                                       const int msgDataType)
  {
    assert(msg != NULL);
    assert(stream != NULL);
    assert(tag != NULL);
    assert(*msg != NULL);

    assert(stream->numtags >= GIMI_MSG_TAGN_CONTROL_MIN ||
           stream->numtags <= GIMI_MSG_TAGN_CONTROL_MAX);
  
    int msgCommand;
    bool result = false;
  
    if (tag == NULL || tag->tag != GIMITAG_COMMAND) {
      return false;
    }
    msgCommand = tag->value;

    // Copy data if there was any.
    tag = bsNextTag(stream);
    if (tag != NULL && tag->tag == GIMITAG_DATA) {
      if (tag->length > 0) {
        (*msg)->datalength = tag->length;
        (*msg)->data = new char[tag->length];
        memcpy((*msg)->data, tag->data, tag->length);
      } else {
        (*msg)->datalength = 0;
        (*msg)->data = NULL;
      }
    }

    switch(msgDataType) {
    case GIMI_PROTOCOL_CONTROL_DATA:
      result = false;
      break;
    case GIMI_PROTOCOL_CONTROL_DATA_SUBSCRIBED:
      //dPrint(ODTEST,"doControlCommand()");
      result = doSubscribedDataCommand(msg, msgUid, msgActFlags, msgCommand);
      break;
    case GIMI_PROTOCOL_CONTROL_SERVICES:
      //dPrint(ODTEST,"doServicesCommand()");
      result = doServicesCommand(msg, msgUid, msgActFlags, msgCommand);
      break;
    case GIMI_PROTOCOL_CONTROL_GENERIC:
      //dPrint(ODTEST,"doControlCommand()");
      result = doControlCommand(msg, msgUid, msgActFlags, msgCommand);
      break;
    default:
      dPrint(ODTEST, "GIMITAG_TYPE contained illegal type.");
      result = false;
      break;
    }
  
    return result;
  }

  bool GIMICore::doDataCommand(GIMIMessage** /* msg */,
                               const uint32_t /* msgUid */,
                               const int /* msgActFlags */,
                               const int /* msgCommand */)
  {
    dPrint(ODTEST, "Called");
    return false;
  }

  bool GIMICore::doSubscribedDataCommand(GIMIMessage **msg,
                                         const uint32_t msgUid,
                                         const int msgActFlags,
                                         const int msgCommand)
  {
    int result = GIMI_ERROR;

    switch (msgCommand) {
    case GIMI_CMD_PING:
      if (msgActFlags == 0) { // Does not request a reply, must be a reply.
        //ownMutex_Lock(subscriberMutex);
        result = subscribers.replyToMessage(msgUid, (*msg)->majorTypeId,
                                            (*msg)->minorTypeId, (*msg)->senderId);
        //ownMutex_Unlock(subscriberMutex);
      }
      break;
    case GIMI_CMD_KEEPALIVE:
      // Refresh subscription updatetime.
      if (!subscriptions.updateSubscriptionTime((*msg)->senderId,
                                                (*msg)->majorTypeId,
                                                (*msg)->minorTypeId))
        {
          dPrint(ODTEST, "Got keepalive for %d:%d from 0x%08x but subscription did "
                 "not exist.", (*msg)->majorTypeId, (*msg)->minorTypeId, (*msg)->senderId);
        } else {
        result = GIMI_OK;
        dPrint(ODTEST, "Updated subscription for %d:%d on 0x%08x",
               (*msg)->majorTypeId, (*msg)->minorTypeId, (*msg)->senderId);
      }
      break;
    default:
      dPrint(ODTEST, "Got subscribed_data control-message, but command was not "
             "recognized (command %d, flags %d)", msgCommand, msgActFlags);  
      break;
    }
    
    if (result == GIMI_OK || result == GIMI_EMPTY)
      return true;
  
    return false;
  }

  bool GIMICore::doServicesCommand(GIMIMessage **msg,
                                   const uint32_t msgUid,
                                   const int msgActFlags,
                                   const int msgCommand)
  {
    int result = GIMI_ERROR;
  
    switch (msgCommand) {
    case GIMI_CMD_SUBSCRIBE:
      
      if (msgActFlags & GIMI_ACT_DOCOMMAND) { // A command
        //ownMutex_Lock(subscriberMutex);
        int replyValue = subscribers.addSubscriber((*msg)->senderId, (*msg)->majorTypeId,
                                                   (*msg)->minorTypeId);
        //ownMutex_Unlock(subscriberMutex);
        result = replyValue;
        if (msgActFlags & GIMI_ACT_REQUESTREPLY) { // Send reply
          int sendResult = sendReplyMessage((*msg)->senderId, GIMI_PROTOCOL_CONTROL_SERVICES,
                                            msgUid, 0, 0, 0, GIMI_CMD_SUBSCRIBE,
                                            replyValue);
          if (sendResult != GIMI_OK) {
            dPrint(ODTEST, "Sending reply to 0x%08x failed with error %d.",
                   (*msg)->senderId, result);
          }
        }
      } else { // A reply
        Lock(KSyncObjectReplyQueue);
        
        int markresult = replyHandler.markDataReplied(msgUid, (*msg)->datalength,
                                                      (*msg)->data);
        Unlock(KSyncObjectReplyQueue);
        Notify(KSyncObjectReplyQueue, KSyncObjectReplyQueue);
        if (markresult != GIMI_REPLYID_WASNOTREPLIED) {
          dPrint(ODTEST, "Got a reply about subscribe from 0x%08x with uid %d but request had likely timed out.",
                 (*msg)->senderId, msgUid);
          result = GIMI_ERROR;
        } else {
          result = GIMI_OK;
        }
      }
      break;
    case GIMI_CMD_UNSUBSCRIBE:
      if (msgActFlags & GIMI_ACT_DOCOMMAND) { // A command
        //ownMutex_Lock(subscriberMutex);
        int replyValue = subscribers.removeSubscriber((*msg)->senderId, (*msg)->majorTypeId,
                                                      (*msg)->minorTypeId);
        //ownMutex_Unlock(subscriberMutex);
        if (msgActFlags & GIMI_ACT_REQUESTREPLY) { // Send reply
          result = sendReplyMessage((*msg)->senderId, GIMI_PROTOCOL_CONTROL_SERVICES,
                                    msgUid, 0, 0, 0, GIMI_CMD_UNSUBSCRIBE,
                                    replyValue);
          if (result != GIMI_OK) {
            dPrint(ODTEST, "Sending reply to 0x%08x failed with error %d.",
                   (*msg)->senderId, result);
          }
        }
      } else { // Is a reply
        Lock(KSyncObjectReplyQueue);
        int markresult = replyHandler.markDataReplied(msgUid, (*msg)->datalength,
                                                      (*msg)->data);
        Unlock(KSyncObjectReplyQueue);
        Notify(KSyncObjectReplyQueue, KSyncObjectReplyQueue);
        if (markresult != GIMI_REPLYID_WASNOTREPLIED) {
          dPrint(ODTEST, "Got a reply about unsubscribe from 0x%08x with uid %d but request had likely timed out.",
                 (*msg)->senderId, msgUid);
          result = GIMI_ERROR;
        } else {
          result = GIMI_OK;
        }
      }
      break;
    case GIMI_CMD_LIST:
      if (msgActFlags & GIMI_ACT_DOCOMMAND) { // Request for a servicelist
        if (msgActFlags & GIMI_ACT_REQUESTREPLY) { // With a request for a reply
          //dPrint(ODTEST, "Request for a servicelist from 0x%08x", (*msg)->senderId);
          result = sendServiceList((*msg)->senderId, msgUid);
        }
      } else { // Reply with a servicelist.
        //dPrint(ODTEST, "Received a servicelist from 0x%08x", (*msg)->senderId);
        if ((*msg)->datalength < 0) { // Servicelist reply should contain data.
          dPrint(ODTEST, "Received servicelist reply from 0x%08x but it had not data!",
                 (*msg)->senderId);
          return false;
        }

        // First; Process the service list right away.
        t_serviceList servicesReceived;
        std::string clientName = ""; // INSERT EMPTY HERE; CALLER MUST TAKE CARE OF FILLING THE TABLE
        processServiceDiscoveryData(servicesReceived,
                                    (*msg)->senderId,
                                    clientName,
                                    (*msg)->data,
                                    (*msg)->datalength);
        
        
        // Now, process based on Uid: If Uid is Zero; this is a spontanius event
        if (msgUid == 0) {
          // Spontaneuous -> To spurious event map; Push new
          dPrintLCRed(ODTEST,"Spontaneous events not supported yet!");
          
        } else {
          // Bundle! -> To bundle map; Must have existing queue to insert.
          bool res = true;
          Lock(KSyncObjectServiceDiscoveryBundleArray);
          TServiceListBundleArray::iterator i = iServiceDiscoveryBundleArray.find(msgUid);
          if (i != iServiceDiscoveryBundleArray.end()) {
            // We found a correct bundle.
            TServiceListArray &slist = i->second;
            slist.push_back(servicesReceived);
            
          } else {
            dPrintLCRed(ODERROR,"Received ServiceReply for Bundle %u from %08x, but not such bundle exists!",
                        msgUid, (*msg)->senderId);
            res = false;
          }
          Unlock(KSyncObjectServiceDiscoveryBundleArray);
          Notify(KSyncObjectServiceDiscoveryBundleArray, KSyncObjectServiceDiscoveryBundleArray);
          return res;
          
        }
      }
      break;
    default:
      dPrint(ODTEST, "Unknown command for services.");
      return false;
      break;
    }
  
    if (result != GIMI_OK) {
      //dPrint(ODTEST, "Service command %d from 0x%08x failed with %d. Parameters were: %d:%d",
      //       msgCommand, (*msg)->senderId, result, (*msg)->majorTypeId, (*msg)->minorTypeId);
      return false;
    }

    return true;
  }

  bool GIMICore::doControlCommand(GIMIMessage **msg,
                                  const uint32_t msgUid,
                                  const int msgActFlags,
                                  const int msgCommand)
  {
    dPrint(16, "Called");

    bool result = false;
    int actionResult = 0;
  
    switch (msgCommand) {
      // We received a ping
    case GIMI_CMD_PING:
      if (msgActFlags & GIMI_ACT_DOCOMMAND) { // Ping command
        if (msgActFlags & GIMI_ACT_REQUESTREPLY) { // requesting reply
          dPrint(7, "Received a ping with a replyrequest from 0x%08x",
                 (*msg)->senderId);
          actionResult = sendReplyMessage((*msg)->senderId, GIMI_PROTOCOL_CONTROL_GENERIC,
                                          msgUid, 0, 0, 0, GIMI_CMD_PING);
          if (actionResult != GIMI_OK) {
            dPrint(6, "Sending ping reply to 0x%08x failed with error %d.",
                   (*msg)->senderId, actionResult);
            result = false;
          } else {
            result = true;
          }
        }
      } else { // A ping reply.
        dPrint(18, "Received a ping reply from 0x%08x with uid %d",
               (*msg)->senderId, msgUid);
        Lock(KSyncObjectReplyQueue);
        actionResult = replyHandler.markPingReplied(msgUid);
        Unlock(KSyncObjectReplyQueue);
        Notify(KSyncObjectReplyQueue, KSyncObjectReplyQueue);

        if (actionResult == GIMI_REPLYID_WASNOTREPLIED) { // Everything went ok.
          result = true;
        } else { // Did not go ok, don't do anything.
          result = false;
        }
      }
      break;
    case GIMI_CMD_SETDEBUGLEVEL:
      if (msgActFlags & GIMI_ACT_DOCOMMAND) { // Command
        debugSetGlobalDebugLvl((*msg)->majorTypeId);
        dPrint(15, "Global debuglevel set to %d", (*msg)->majorTypeId);
        result = true;
      }
      break;
    default:
      dPrint(20, "Unrecognized CONTROL-command.");
      return result;
    }
  
    return result;
  }

}
